#include "Pathfinder.h"

namespace Pathing
{
	Pathfinder::Pathfinder(Waypoint::Vector_t& waypoints) : m_waypoints(waypoints), m_currentWaypoint(0)
	{
	}
	unsigned int Pathfinder::CurrentWaypoint()
	{
		return m_currentWaypoint;
	}
	void Pathfinder::CurrentWaypoint( unsigned int& currentWaypoint )
	{
		m_currentWaypoint = currentWaypoint;
	}
	Subject<Maths::Vector3D>& Pathfinder::getPosition()
	{
		return m_position;
	}
	Maths::Vector3D Pathfinder::getDirection()
	{
		Maths::Vector3D direction;
		Maths::Vector3D position = m_position.getObject();
		Pointer<Waypoint> waypoint = m_waypoints[m_currentWaypoint];
		
		if (position.DistanceSqr(waypoint->Location()) < waypoint->Radius() * waypoint->Radius())
			waypoint = m_waypoints[++m_currentWaypoint];
		
		direction = m_waypoints[m_currentWaypoint]->Location() - position;

		return direction;
	}
}